#include "CPilot.h"

#include <mrpt/random/RandomGenerators.h>
CPilot::CPilot() {
	// angles de consigne
	phi_c = 0.0;
	theta_c = 0.0;
	psi_c = 0.0;

	cur_wpt = 0;
	etape = 0;// étape de navigation

	// Estimation de la position
	pose_e = CPose3D(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); // position estimée en cartésiennes sur la carte (x,y,z,yaw,pitch,roll)
	v_e = TPoint3D(0.0, 0.0, 0.0); // vitesse estimée
	tour = -1;

	APloc.reconrepere.ajouterRepere(GPoint2D((float) -5, (float) -1));
	APloc.reconrepere.ajouterRepere(GPoint2D((float) 5, (float) -1));
	//APloc.reconrepere.ajouterRepere(GPoint2D((float) -3, (float) 3));
	//APloc.reconrepere.ajouterRepere(GPoint2D((float) 3, (float) -3));
	compteur=0;
	compteur2=1;
	compteur2=0;
	perfprec=10000000;
	perftest=0;
}

CPilot::~CPilot() {

}
inline float square(float a){return a*a;}
void CPilot::estimation(CObservation2DRangeScan scan, CPose3D pose_m) {
	conversionMrptTypeScan(scan, &(scanCalao));//à récupérer immédiatement aprés le calcul du télémètre
	APloc.loc(scanCalao, 481, 1, pose_m.pitch(), pose_m.roll(), pose_m.yaw(),
			pose_m.z(), 0.40, 1, 2.0);
	v_e.x=pose_m.x()-pose_e.x();// cf fin de fonction
	//cout << "estim" << APloc.pose_m.x() << "\t" << APloc.pose_m.y() << endl;
	//cout << "reel" << pose_m.x() << "\t" << pose_m.y() << endl;
	//cout << "filt" << APloc.estimateur.pose_e.x() << "\t" << APloc.estimateur.pose_e.y() << endl;
	//cout << "err" << APloc.pose_m.x()-pose_m.x() << "\t" << APloc.pose_m.y()-pose_m.y() << endl;
	//cout << "errfilt" << APloc.estimateur.pose_e.x()-pose_m.x() << "\t" << APloc.estimateur.pose_e.y()-pose_m.y() << endl;
	/*if(compteur3<10000)
	{
		compteur3++;
	}else
	{
		if(compteur<100){
				perftest+=square(APloc.estimateur.pose_e.x()-pose_m.x())+square(APloc.estimateur.pose_e.y()-pose_m.y());
				perftest+=2*(square(APloc.estimateur.v_e.x-v_e.x)+square(APloc.estimateur.v_e.y-v_e.y));
				compteur++;
			}else{
				compteur=0;
				if(perftest>perfprec){// si j'ai empiré je reviens en arrière
					cout << "pas bon (kx,kv)=" <<  APloc.estimateur.kx << " "<< APloc.estimateur.kv << " err " << perftest << endl;
					if(compteur2%2){
						APloc.estimateur.kx-=kxincr;
					}
					else
					{
						APloc.estimateur.kv-=kvincr;
					}
					perfprec+=100.0/compteur2;
				}else{
					compteur2++;// si je me suis amélioré j'augmente compteur
					cout << "mieux: kx=" << APloc.estimateur.kx << " kv=" << APloc.estimateur.kv << " err " << perftest << endl;
					perfprec=perftest;
				}
					// marche aléatoire
					if(compteur2%2){
						kxincr=(float) mrpt::random::RandomUni(-0.2/sqrt(compteur2),0.2/sqrt(compteur2));
						APloc.estimateur.kx+=kxincr;
					}
					else{
						kvincr=(float) mrpt::random::RandomUni(-0.2/sqrt(compteur2),0.2/sqrt(compteur2));
						APloc.estimateur.kv+=kvincr;
					}
					perftest=0;
			}
	}
	//*/
	pose_e.x(pose_m.x());// mémorise la position pour plus tard
}

void CPilot::piloteHuit(CObservation2DRangeScan scan, CPose3D pose_m,
		TPoint3D v_m) {
	//pilotage
	double psic_rate;
	//APpilotehuit.piloteHuit(APloc.pose_m, APloc.v_m, APloc.pose_m.yaw(),APloc.yaw_offset, 1.5, 5,.5,.5, 2, 1.5, 8*M_PI/180.0);
	//APpilotehuit.piloteWpt(APloc.pose_m, APloc.v_m,APloc.yaw_offset);
	APpilotehuit.piloteHuit(APloc.estimateur.pose_e, APloc.estimateur.v_e,APloc.pose_m.yaw(),APloc.yaw_offset,1.5,5,.5,.5,2,1.5,8*M_PI/180.0);
	/*APpilotehuit.piloteZero(
			APloc.estimateur.pose_e,
			APloc.estimateur.v_e,
			APloc.yaw_offset,
				0.15,
				0.1,
				0.4,
				0,
				1,
				(float) (APloc.analysescan.dt()/1000.0),
				6);//*/
	/*
			GPose3D pose_m,
			GPoint3D v_m,
			float yaw_offset,
			float kp,
			float ki,
			float kd,
			float e_int_sat,
			float s_int_sat,
			float dt,
			float ang_max_deg */
	/*APpilotehuit.piloteWpt(
			APloc.pose_m,
			APloc.estimateur.v_e,
			APloc.yaw_offset,
			17878,
			1548784789,
			1851545,
			15148,
			1885,
			(float) (APloc.analysescan.dt()/1000.0),
			6);//*/
	APpilotehuit.piloteWpt2(
				APloc.pose_m,
				APloc.estimateur.v_e,
				APloc.yaw_offset,
				.2,
				0.01,
				1,
				3,
				.02,
				APloc.analysescan.dtfloat(),
				6);
	psi_c = APpilotehuit.getPsiC();
	phi_c = APpilotehuit.getPhiC();
	theta_c = APpilotehuit.getThetaC();
	//*/
}

void CPilot::piloteWpt(CPose3D pose_m, TPoint3D v_m) {
	psi_c = waypts[cur_wpt].z;

	TPoint3D pos(pose_m[0], pose_m[1], 0);
	TPoint3D next(waypts[cur_wpt].x, waypts[cur_wpt].y, 0);
	TPoint3D dir = next - pos;

	if (dir.norm() < 0.5) {
		cur_wpt++;
		if (cur_wpt == waypts.size()) {
			cur_wpt = 0;
		}
		return;
	}

	double vec_rot = atan2(dir.y, dir.x) - pose_m.yaw();

	double vx = 5 * M_PI / 180 * (1.2 * v_m.x - dir.x);
	double vy = -5 * M_PI / 180 * (1.2 * v_m.y - dir.y);
	theta_c = vx * cos(pose_m.yaw()) + vy * sin(-pose_m.yaw());
	phi_c = vx * sin(pose_m.yaw()) + vy * cos(pose_m.yaw());
	if (phi_c > 5 * M_PI / 180)
		phi_c = 5 * M_PI / 180;
	if (phi_c < -5 * M_PI / 180)
		phi_c = -5 * M_PI / 180;

	if (theta_c > 5 * M_PI / 180)
		theta_c = 5 * M_PI / 180;
	if (theta_c < -5 * M_PI / 180)
		theta_c = -5 * M_PI / 180;

}

double CPilot::getPhiC() {
	return phi_c;
}

double CPilot::getThetaC() {
	return theta_c;
}
double CPilot::getPsiC() {
	return psi_c;

}

void CPilot::generateHuitWpts() {
	waypts.clear();
	waypts.push_back(TPoint3D(3, 1, -45 * M_PI / 180));
	waypts.push_back(TPoint3D(5, 1.5, -100 * M_PI / 180));
	waypts.push_back(TPoint3D(6.5, 0.5, -135 * M_PI / 180));
	waypts.push_back(TPoint3D(6.5, -0.5, -180 * M_PI / 180));
	waypts.push_back(TPoint3D(5, -1.5, -230 * M_PI / 180));
	waypts.push_back(TPoint3D(3, -1, -260 * M_PI / 180));
	waypts.push_back(TPoint3D(0, 0, -290 * M_PI / 180));
	waypts.push_back(TPoint3D(-3, 1, -200 * M_PI / 180));
	waypts.push_back(TPoint3D(-5, 1.5, -120 * M_PI / 180));
	waypts.push_back(TPoint3D(-6.5, 0.5, -60 * M_PI / 180));
	waypts.push_back(TPoint3D(-6.5, -0.5, 0 * M_PI / 180));
	waypts.push_back(TPoint3D(-5, -1.5, 60 * M_PI / 180));
	waypts.push_back(TPoint3D(-3, -1, 120 * M_PI / 180));
	waypts.push_back(TPoint3D(0, 0, 60 * M_PI / 180));

}

void CPilot::generateStaticWpts() {
	waypts.clear();
	waypts.push_back(TPoint3D(0.1, 0, 90* M_PI / 180));
	waypts.push_back(TPoint3D(-0.1, 0, -90 * M_PI / 180));
}

TPoint3D CPilot::getCurrentWpt() {

	if (waypts.size() > 0) {
		return waypts[cur_wpt];

	} else {
		return TPoint3D(0, 0, 0);
	}

}
